#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <queue>
#include <deque>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv/cv.h>
/////////////////services
#include "robina_facerecognition/StoreTemplateface.h"
#include "robina_facerecognition/loadTemplateface.h"
#include "robina_facerecognition/recognizeFaceOnce.h"
#include "robina_facerecognition/stopContiniouslyfaceDetection.h"
#include "robina_facerecognition/facesAngles.h"
#include "robina_facerecognition/imageResponse.h"
#include <robina_facerecognition/ac_faceRecognitionAction.h>
#include <robina_facerecognition/ac_faceRecognitionContiniouslyAction.h>
#include <robina_facerecognition/deleteFaces.h>
#include <robina_facerecognition/waitToDetectFace.h>
#include <robina_facerecognition/saveImage.h>
////pantilt
#include <robina_facerecognition/pantilt.hpp>
////////////////actions
#include <actionlib/server/simple_action_server.h>

#include "robina_facerecognition/Face_Angle.h"
#include "robina_facerecognition/Face_Angles.h"
#include "robina_facerecognition/faceRecognition_msg.h"
#include <robina_facerecognition/LuxandFaceSDK.h>

///////////////openCV
#include "sensor_msgs/image_encodings.h"
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>



#include "LuxandFaceSDK.h"
#include <boost/signals2/mutex.hpp>
#define  SERIAL_KEY "NTAmlOelBiP3Nm+yeJV/LwwH6yEYNuhndErN69DMzewzUV5RElzKIqSA6NE2YHCkyLzzK9OvVAW0qpu31iDeXvgckg25pmkOO7o67pv9rDLeNSzCzubasJ7vwoJQxqHtZESWYzAS3AxsNNrXM8KdUUiUxyi0R790gKEyqCAanuo="

using namespace std;
using namespace cv;
static const std::string OPENCV_WINDOW = "Image window";
static const std::string SDK_ERROR = "Please run the License Key Wizard (Start - Luxand - FaceSDK - License Key Wizard)\n EdetectFaceor activating FaceSDK";
static const std::string SDK_OK = "SDK has been activated!";
typedef struct _FaceTemplate
{
    vector <FSDK_FaceTemplate> templateData;
    string Name;
    vector <string>  nameVector;

}TfaceTemplate;

typedef actionlib::SimpleActionServer<robina_facerecognition::ac_faceRecognitionAction> faceRecognitionAction;
typedef actionlib::SimpleActionServer<robina_facerecognition::ac_faceRecognitionContiniouslyAction> faceRecognitionContiniouslyAction;


class faceRecognition
{

public:
    faceRecognition();
    ~faceRecognition();

    int init();
private:
    ///services
    bool storeTempCB (robina_facerecognition::StoreTemplateface::Request &req,  robina_facerecognition::StoreTemplateface::Response &res );
    bool loadTempCB (robina_facerecognition::loadTemplateface::Request &req,   robina_facerecognition::loadTemplateface::Response &res );
    bool recognizeOnceCB(robina_facerecognition::recognizeFaceOnce::Request & req , robina_facerecognition::recognizeFaceOnce::Response &res );
    bool facesAnglesCB(robina_facerecognition::facesAngles::Request &req, robina_facerecognition::facesAngles::Response &res);
    bool stopContiniouslyCB(robina_facerecognition::stopContiniouslyfaceDetection::Request &req, robina_facerecognition::stopContiniouslyfaceDetection::Response &res);
    bool imageResponseCB(robina_facerecognition::imageResponse::Request &req, robina_facerecognition::imageResponse::Response &res);
    bool deleteFacesCB(robina_facerecognition::deleteFaces::Request &req,robina_facerecognition::deleteFaces::Response &res);
    bool waitForDetectFaceCB(robina_facerecognition::waitToDetectFace::Request &req,robina_facerecognition::waitToDetectFace::Response &res);
    bool saveImageCB(robina_facerecognition::saveImage::Request  &req,robina_facerecognition::saveImage::Response &res);

    ///actions
    void faceDetectionActionExecuteCB(const robina_facerecognition::ac_faceRecognitionGoalConstPtr goal);
    void faceDetectionContiniouslyActionExecuteCB(const robina_facerecognition::ac_faceRecognitionContiniouslyGoalConstPtr goal);

    actionlib::SimpleActionServer<robina_facerecognition::ac_faceRecognitionAction> *faceRecognition_as;
    actionlib::SimpleActionServer<robina_facerecognition::ac_faceRecognitionContiniouslyAction> *faceRecognitionContiniously_as;

    void saveXml( TfaceTemplate vectorFaces );
    bool loadXml();
    void imageCB( const sensor_msgs::ImageConstPtr& msg );
    bool setPantilt(int tilt);

    int countOfPeople;

    void checkXmlFile();
    void trainFileDataPath();

    ros::NodeHandle nh;
    ros::Rate loop_rate;

    ros::ServiceServer srvStoreTemp_srv;
    ros::ServiceServer srvLoadTemp;
    ros::ServiceServer srvRecognizeOnce;
    ros::ServiceServer srvRecognizeContiniously;
    ros::ServiceServer srvImageResponse;
    ros::ServiceServer srvStopContinouslyDetectFace;
    ros::ServiceServer srvDeleteFaces;
    ros::ServiceServer srvWaitToDetectFace;
    ros::ServiceServer srvSaveImage;



    ros::Publisher face_detection_pub;

    bool stopContiniouslyAction;
    bool bufferDone;
    bool waitForDetectFace(int tryNumber);
    cv::Mat image;
    IplImage *saveImage;
    sensor_msgs::Image responseImage;
    queue <HImage> buffer;
    HImage   imageHandle;

    TfaceTemplate recognizeVectorFaces;
    TfaceTemplate vectorFaces;
    TfaceTemplate loadVectorName;

    int personNumber;
    int nPersons ;
    int i;
    int NamevectorSize;
    int finalVectorSize;

    char personName[255];
    char npersonAddress[255];
    CvFileStorage *personnumber;

    string train_file_path;
    char xmlAdress[255];
    string saveAddress;


    sensor_msgs::ImageConstPtr  msg;
    boost::signals2::mutex mutex;
};
